#include "lidar.hpp"

Lidar::Lidar(std::shared_ptr<ros::NodeHandle> node_handle_ptr,
             const std::string &serial_port)
    : node_handle_ptr_(node_handle_ptr), Serial(serial_port) {
    name_ = serial_port.substr(5);
    //   publisher_ = node_handle_ptr_->advertise<sensor_msgs::Lidar>(name_, 1);
    switch_ =
        node_handle_ptr_->advertiseService(name_, &Lidar::SwitchCallBack, this);
    //   LOG(ERROR) << name_ << " imu frame size:(" << sizeof(LidarFrame) <<
    //   ")";
    SetRunning(false);
}

void Lidar::LidarRxThread() {
    ros::Rate rate(kLidarFrequence);
    std::vector<uint8_t> rx_data;
    LOG(ERROR) << name_ << " thread started!";
    while (true) {
        // std::unique_lock<std::mutex> lock(mutex_);
        if (is_running_ && ReadFromIO(rx_data))
            ParseData(rx_data);
        rate.sleep();
    }
}

void Lidar::ParseData(std::vector<uint8_t> &data) {

    // LOG(ERROR) << "Data size:(" << data.size() << ").";
    auto get_frame_crc = [&](const std::vector<uint8_t> &data) {
        uint16_t summary = 0;
        if (data.size() < 280) {
            LOG(ERROR) << "Data size less than frame size.";
            return summary;
        }
        uint16_t count = uint16_t(data[3]) << 8 | uint16_t(data[2]);
        uint16_t start = uint16_t(data[5]) << 8 | uint16_t(data[4]);
        uint16_t scope = uint16_t(data[7]) << 8 | uint16_t(data[6]);
        LOG(ERROR) << "count(" << size_t(count) << "),start:(" << size_t(start)
                   << "),scope:(" << size_t(scope) << ")";
        summary = count + start + scope;
        for (size_t i = 0; i < size_t(count); i++) {
            summary += uint16_t(data[i * 3 + 8]);
            summary +=
                uint16_t(data[i * 3 + 10]) << 8 | uint16_t(data[i * 3 + 9]);
            // LOG(ERROR) << "i:(" << i << "),size(" << data.size() << ")";
        }
        return summary;
    };
    while (data.size() >= sizeof(LidarStatusFrame)) {
        size_t drop = 0;
        while (data.size() >= sizeof(LidarStatusFrame)) {
            if ((data[0] == 0xCF && data[1] == 0xFA) ||
                (data[0] == 0x53 && data[1] == 0x54 && data[6] == 0x45 &&
                 data[7] == 0x44)) {
                break;
            } else {
                printf("%02x ", data[0]);
                data.erase(data.begin());
                drop++;
            }
        };
        if (drop > 0)
            LOG(ERROR) << name_ << " drop size:(" << drop << "),data size:("
                       << data.size() << ").";
        if (data.size() < sizeof(LidarStatusFrame))
            return;
        if (data[0] == 0xCF) {
            size_t count = int(uint16_t(data[3]) << 8 | uint16_t(data[2]));
            size_t frame_size = sizeof(LidarFrameHead) +
                                count * sizeof(LidarData) +
                                sizeof(LidarFrameTail);
            if (count != 90 && count != 135) {
                LOG(ERROR) << name_ << " error count:(" << count
                           << "),frame size:(" << frame_size << ").";
                data.erase(data.begin(),
                           data.begin() + sizeof(LidarStatusFrame));
                break;
            }
            if (data.size() < frame_size)
                return;
            std::vector<uint8_t> lidar_data(data.begin(),
                                            data.begin() + frame_size);
            data.erase(data.begin(), data.begin() + frame_size);
            uint16_t crc = uint16_t(lidar_data[frame_size - 1]) << 8 |
                           uint16_t(lidar_data[frame_size - 2]);
            if (get_frame_crc(lidar_data) != crc) {
                LOG(ERROR) << name_ << " frame crc error.";
                continue;
            } else {
                LOG(ERROR) << name_ << " frame crc successful.";
            }
        }
        if (data[0] == 0x53) {
            LOG(ERROR) << "Receive status frame,data size:(" << data.size()
                       << "),status frame size:(" << sizeof(LidarStatusFrame)
                       << ")";
            printf("Receive status frame.\n");
            for (auto &&byte : data) {
                printf("%02x ", byte);
            }
            printf("\n");
            data.erase(data.begin(), data.begin() + sizeof(LidarStatusFrame));
        }
    };
    LOG(ERROR) << "================================size:(" << data.size()
               << ")";
}

// void Lidar::Publish(const sensor_msgs::Imu &imu) {
//   publisher_.publish(imu);
//   static auto last = ros::Time::now();
//   last = imu.header.stamp;
//   times_.push_back(last);
//   while (times_.size() > 100) {
//     times_.erase(times_.begin());
//     LOG_EVERY_N(ERROR, 100)
//         << name_ << " publish frequence in last 100 message:("
//         << double(times_.size() - 1) /
//                ((times_.back() - times_.front()).toSec())
//         << ")hz.";
//   }
// }

bool Lidar::SetRunning(const bool &running) {
    LOG(ERROR) << "SetRunning:(" << running << "),is_start:(" << is_start_
               << ")";
    if (running) {
        Open(kBandrateCode921600);
        if (WriteToIO(kLidarStartCommand))
            LOG(ERROR) << name_ << " Send kLidarStartCommand successful.";
        else
            LOG(ERROR) << name_ << " Send kLidarStartCommand failure.";
        is_running_ = true;
        return true;
    } else {
        if (WriteToIO(kLidarStopCommand))
            LOG(ERROR) << name_ << " Send kLidarStopCommand successful.";
        else
            LOG(ERROR) << name_ << " Send kLidarStopCommand failure.";
        Close();
        is_running_ = false;
        return true;
    }
}

bool Lidar::SwitchCallBack(std_srvs::SetBool::Request &req,
                           std_srvs::SetBool::Response &res) {
    if (SetRunning(req.data)) {
        res.success = true;
        return true;
    } else {
        res.success = false;
        return false;
    }
}